#include"distance_filter.h"
#include<math.h>

Distance_filter::Distance_filter(PredictMove* kalman1)
{
//    kalman1->SetPredictError(0.01);
//    kalman1->SetComputeError(1);
    kalman=kalman1;
}

double oushi_distance(Point a, Point b ){
    return sqrt((a.x-b.x)*(a.x-b.x) + (a.y-b.y)*(a.y-b.y));
}

double Distance_filter::filting(double dis0)
{
    if(dists.size()<2)
    {
        dists.push_back(dis0);
        return dis0;
    }
    double dis_sum1=0;
    double dis_sum2=0;
    double speed = 0;

    int begin=0;
    if(dists.size()%2 == 1) begin=1;
    if(dists.size() >= 2)
    {
        for(int i = begin ; i < dists.size() / 2+begin; i++)
        {
            dis_sum1 += dists.at(i);
        }

        for(int i = dists.size() / 2 +begin ; i < dists.size() ; i++)
        {
            dis_sum2 += dists.at(i);
        }
        speed = (dis_sum2 - dis_sum1)/(dists.size() / 2 )/(dists.size() / 2);
    }

    double speed_sum1=0,speed_sum2=0;
    double a_speed=0;
//    if(fabs(speed)>5)
//    {
//        int flag=(dis0-dists.back())>0 ? 1:-1;
//        speed=flag*5;
//    }
    if(Speeds.size()==0)
    {
        Speeds.push_back(speed);
    }
    if(Speeds.size()>=1)
    {
        for(int i = begin ; i < Speeds.size() / 2+1; i++)
        {
            speed_sum1 += Speeds.at(i);
        }

        for(int i = Speeds.size() / 2 +1 ; i < Speeds.size() ; i++)
        {
            speed_sum2 += Speeds.at(i);
        }
        speed_sum2+=speed;
        if(dists.size()%2 == 0)
            a_speed = (speed_sum2 - speed_sum1)/(Speeds.size() / 2 )/(Speeds.size() / 2);
        else
            a_speed = (speed_sum2 - speed_sum1)/(Speeds.size() / 2 +1)/(Speeds.size() / 2+1);

    }

//    if(fabs(dis0-dists.back())>speed*2)
//    {

//    }

    if(Speeds.size()<max_size)
    {
        Speeds.push_back(speed);
    }
    else
    {
        vector<double>::iterator first = Speeds.begin();
        Speeds.erase(first);
        Speeds.push_back(speed);
    }


    double  ac_dis;
    if(fabs(dis0-dists.back())>fabs(speed)*2+5)
    {
        int flag_now=(dis0-dists.back())>0 ? 1:-1;
        int flag_speed=Speeds.back()>0? 1:-1;
        if(flag_now*flag_speed==1)
        {
           ac_dis=dists.back()+(speed*2+5);
        }
        else
        {
            double diff=fabs((dis0-dists.back())/3)<30 ? ((dis0-dists.back())/3):(30*flag_now);
            ac_dis=dists.back()+diff;
        }


        if(dists.size()<max_size)
        {
            dists.push_back(ac_dis);
        }
        else
        {
            vector<double>::iterator first = dists.begin();
            dists.erase(first);
            dists.push_back(ac_dis);
        }
    }
    else
    {
        ac_dis=dis0;
        if(dists.size()<max_size)
        {
            dists.push_back(dis0);
        }
        else
        {
            vector<double>::iterator first = dists.begin();
            dists.erase(first);
            dists.push_back(dis0);
        }
    }


    return ac_dis;
}

double Distance_filter::calc_distance(vector<Point> _armor_points, int my_color)
{
    double length = max(oushi_distance(_armor_points[0], _armor_points[1]),
                        oushi_distance(_armor_points[3], _armor_points[2]));
    double excel_dis = 0;
    if(my_color == 1) //hit red
    {
        excel_dis = filting(55614.111 * pow(length, -0.85734119)) ;
    }
    else if(my_color == 2)
    {
        excel_dis = filting(103728.161 * pow(length, -0.98201335)) ;
    }



    vector<double> nowpoints = kalman->GetnowPoint((double)excel_dis,(double)excel_dis);
    float final_dis = nowpoints[0];


    return final_dis;

}
